#include <stdio.h>
#include <sys/time.h>
#include <unistd.h>
#include <signal.h>

#include "ethercattype.h"
#include "nicdrv.h"
#include "ethercatbase.h"
#include "ethercatmain.h"
#include "ethercatdc.h"
#include "ethercatcoe.h"
#include "ethercatfoe.h"
#include "ethercatconfig.h"
#include "ethercatprint.h"
#include "math.h"
#include <alchemy/task.h> 
#include <alchemy/timer.h> 

#define TASK_PRIO 99 /* Highest RT priority */ 
#define TASK_MODE 0 /* No flags */ 
#define TASK_STKSZ 0/* Stack size (use default one) */ 
#define TASK_PERIOD  1000000  /* 100 usc period */
#define NSEC_PER_SEC 1000000000
 RT_TASK task_desc; 

char IOmap[4096];
uint16 control;

uint16 control_world;
uint16 state;
int32 curr_position;
int32 position=0;
int32 position2;
int add_position;
int i=0;
RTIME tt=0;
int64 toff,gl_delta;
// 使从站进入op状态
void slavetop(int i)
{
   ec_send_processdata();
	ec_receive_processdata(EC_TIMEOUTRET);
	ec_slave[i].state = EC_STATE_OPERATIONAL;
	ec_writestate(0);
  // ec_statecheck(0, EC_STATE_OPERATIONAL,  EC_TIMEOUTSTATE);
}
void position_to_add(int32 position)
{
	
	ec_slave[0].outputs[0x0002] = position;
	ec_slave[0].outputs[0x0003] = (position>>8)&0xff;
	ec_slave[0].outputs[0x0004] = (position>>16)&0xff;
	ec_slave[0].outputs[0x0005] = (position>>24)&0xff;
	
}
void ec_sync1(int64 reftime, int64 cycletime , int64 *offsettime)
{
   static int64 integral = 0;
   int64 delta;
   /* set linux sync point 50us later than DC sync, just as example */
   delta = (reftime-500000) % cycletime;
   if(delta>(cycletime / 2)) 
   { delta= delta - cycletime; }
   if(delta>0){ integral++; }
   if(delta<0){ integral--; }
   *offsettime = -(delta /20) - (integral /10);
  // rt_printf("%lld\n",delta); 
} 
void ec_sync(int64 reftime, int64 cycletime , int64 *offsettime)
{
   static int64 integral = 0;
  
   int64 delta;
   
   /* set linux sync point 50us later than DC sync, just as example */
   delta = (reftime-500000) % cycletime;
   if(delta>(cycletime / 2)) 
   { delta= delta - cycletime; }
  *offsettime = -(delta /20) - (integral /10000);
   integral=integral+delta;
   //d=d-delta; 
  //rt_printf("%lld\n",delta); 
} 
void add_timespec(struct timespec *ts, int64 addtime)
{
   int64 sec, nsec;
   
   nsec = addtime % NSEC_PER_SEC;
   sec = (addtime - nsec) / NSEC_PER_SEC;
   ts->tv_sec += sec;
   ts->tv_nsec += nsec;
   if ( ts->tv_nsec > NSEC_PER_SEC ) 
   { 
      nsec = ts->tv_nsec % NSEC_PER_SEC;
      ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC;
      ts->tv_nsec = nsec;
   }   
}   


void simpletest(void *arg)
{        float t=0;
        char* ifname="rteth0";
	if(ec_init(ifname))
	{
	   rt_printf("start ethernet at %s\n",ifname);
	   if ( ec_config_init(FALSE) > 0 )
	   {              
			 rt_printf("found %d slave on the bus\n",ec_slavecount);
                       //ec_slave[1].PO2SOconfig = &SDOSetting;
                          
                         ec_config_map(&IOmap);
                         ec_configdc(); 
                         ec_dcsync0(1, TRUE,1000000,200000);
                        
                        
                        
			for(i=0;i<ec_slavecount;i++)
			{      
                        
                         slavetop(i); 
                     
                         rt_printf("slave%d to op\n", i);
                              
			}
                             
			if(ec_slave[0].state == EC_STATE_OPERATIONAL)
             { 
                        
                          
                         ec_slave[0].outputs[0x0000] = 0x00;
			  ec_slave[0].outputs[0x0001] = 0x00;
                        // ec_send_processdata(); 
                        // ec_receive_processdata(EC_TIMEOUTRET);
                        // rt_timer_spin(95000000); 
                          
                        
                         ec_slave[0].outputs[0x0000] = 0x06;
			 ec_slave[0].outputs[0x0001] = 0x00; 
                         //ec_send_processdata(); 
                         //ec_receive_processdata(EC_TIMEOUTRET);
                         ec_slave[0].outputs[0x000c] = 0x08;
                        // rt_timer_spin(90000000);
                       // uint8 moder= 8;
	                   // ec_SDOwrite(1, 0x6060, 0x00, FALSE, sizeof(moder), &moder, EC_TIMEOUTRXM);
                         
                         //ec_send_processdata(); 
                         //ec_receive_processdata(EC_TIMEOUTRET);
                           rt_timer_spin(9000000);

                        ec_slave[0].outputs[0x0000] = 0x07;
                        ec_slave[0].outputs[0x0001] = 0x00;
                        //ec_send_processdata();  
                        //ec_receive_processdata(EC_TIMEOUTRET);
                         // rt_timer_spin(90000000); 
                            
                        ec_slave[0].outputs[0x0000] = 0x0f;
                        ec_slave[0].outputs[0x0001] = 0x00;
                        //ec_send_processdata();
                       // ec_receive_processdata(EC_TIMEOUTRET);
		            

                         struct timespec   ts;
                       
                         clock_gettime(CLOCK_MONOTONIC, &ts);
                          toff = 0;
                       // ec_send_processdata();
                          //int err = rt_task_set_periodic(NULL,TM_NOW, TASK_PERIOD);
                          int64 max=0;
                              
                while(1)
		        {  
                         add_timespec(&ts, TASK_PERIOD + toff);
                         clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts,NULL);
                          //rt_printf("operation enable\n");
                               
                                 
                              //position=(int32)100000*sin(t);
                                   // t=t+0.0002;
                               // position=1000;
                             //  rt_printf("%d\n",position); 
                           
		  // ec_SDOwrite(1, 0x6060, 0x00, FALSE,sizeof(position),&position, EC_TIMEOUTRXM);
                           
                               
                                 position=(int32)(10000*sin(t));
                                 t=t+0.001;
                                 position_to_add( position);
                               
                               
                                ec_sync(ec_DCtime,1000000,&toff);
                               
                                 ec_send_processdata();
				                    ec_receive_processdata(EC_TIMEOUTRET);
                                //RTIME time2 = rt_timer_read();
                               // RTIME time3 = time2 - time1;
                                
                                 // position=position+100;
                                  
                                   
                                    
                                  // int64 zyj=ec_DCtime%1000000;
                                  //rt_printf("%lld\n",zyj); 
                               //int64 zyj=ec_DCtime-max;
                                   //int64 aa=zyj;
                                     //if(zyj>max)
                                        // {
                                      //rt_printf("%lld\n",zyj);
                                             //max=ec_DCtime;
                                        // }
                             
                                 
                                //tt=time1;
						
				//	  int8 feedb_position;
//int       size_position=sizeof(feedb_position);				                     
                //   ec_SDOread(1,0x6061,0x00,FALSE,&size_position,&feedb_position,EC_TIMEOUTRXM);
			//int32 abc =feedb_position-position;
		 rt_printf("%lld\n", position);                  
			//}
					
                                    
                        
                  
                             
					
                                      
                      
					//rt_task_wait_period(NULL);
                                  
                                        
		      }
	           }
			
	 }
	
       }
 
		

}

int creat_task(void)
{
                  
              int err; 
            
             
             /* Create a real-time task */ 
            int err_create =rt_task_create(&task_desc,"task_desc",TASK_STKSZ,TASK_PRIO,TASK_MODE); 
              if (err_create!=0)
			  {
				  printf("创建进程错误");
			  } 
             /* If successfully created, start thetask. */ 
           int err_start=rt_task_start(&task_desc,&simpletest,NULL); 
		   if (err_start!=0)
		   {
			  printf("开启任务失败");
		   }
		   if((err_start||err_create)==0)
		   {
              return 0;
		   }
		   else
		   {
			   return -1;
		   }
	 
	      
           
}
int main(int argc, char *argv[])
{
       
	printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n");
	
    
	if (argc > 1)
	{      
               int error=creat_task();
                    if(error<0)
		{
			printf("创建xenomai建成错误");
		}
		
		
               
		
		   
	}
	else
	{
		printf("Usage: simple_test ifname1\nifname = eth0 for example\n");
	}   
               pause();
	printf("End program\n");
	return 0;
}


